/*
 * @Author: xiaoxiaotaoya 1712331226@qq.com
 * @Date: 2024-12-18 21:50:37
 * @LastEditors: xiaoxiaotaoya 1712331226@qq.com
 * @LastEditTime: 2024-12-22 17:22:29
 * @FilePath: /trobot_ws/src/controller_modules/mc_path_planner/src/complete_coverage_path/ccpp_base.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <mc_path_planner/complete_coverage_path/ccpp_base.h>

namespace ccpp{

CoveragePlannerBase::CoveragePlannerBase(boost::shared_ptr<MapBase> map_handler_ptr, boost::shared_ptr<SmoothBase> smooth_ptr){
    mapHandlerPtr_ = map_handler_ptr;
    smoothPathPtr_ = smooth_ptr;
    debugDir_ = "/home/xxty/trobot/trobot_ros/trobot_ws/output/ccpp/";
    debug_mode_ = true;
}

CoveragePlannerBase::~CoveragePlannerBase(){
    
}

// 字符串转换
string CoveragePlannerBase::toString(PathType path_type){
    string type_str = "UNKNOWN";
    switch(path_type){
        case PathType::EDGE_PATH:
            type_str = "EDGE_PATH";
            break;
        case PathType::BOW_SHAPED_PATH:
            type_str = "BOW_SHAPED_PATH";
            break;
        case PathType::ARC_SHAPED_PATH:
            type_str = "ARC_SHAPED_PATH";
            break;
        case PathType::SPRIAL_AND_BOW_SHAPED_PATH:
            type_str = "SPRIAL_AND_BOW_SHAPED_PATH";
            break;
        case PathType::SPRIAL_AND_ARC_SHAPED_PATH:
            type_str = "SPRIAL_AND_ARC_SHAPED_PATH";
            break;        
        default:
            break;
    }
    return type_str;
}

// 计算距离 cv::Point
double CoveragePlannerBase::calculateDistance(cv::Point p1, cv::Point p2){
    double dx = p2.x - p1.x;
    double dy = p2.y - p1.y;
    return hypot(dx, dy);
}

// 计算距离Point2f
double CoveragePlannerBase::calculateDistance(const Point2f& p1, const Point2f& p2) {
    return std::sqrt(std::pow(p2.x - p1.x, 2) + std::pow(p2.y - p1.y, 2));
}

// 根据起点和终点生成等间距的点
std::vector<Point2f> CoveragePlannerBase::generateLinePoints(const Point2f& start, const Point2f& end, double interval){
    std::vector<Point2f> points;

    // 计算直线的总长度
    double totalDistance = calculateDistance(start, end);

    // 计算需要生成的点数
    int numPoints = static_cast<int>(std::ceil(totalDistance / interval));

    // 方向向量
    double dx = (end.x - start.x) / totalDistance;
    double dy = (end.y - start.y) / totalDistance;

    // 生成点
    for (int i = 0; i <= numPoints; ++i) {
        double x = start.x + i * interval * dx;
        double y = start.y + i * interval * dy;
        points.emplace_back(Point2f(x, y));
    }

    return points;
}

// 稀疏点直接插值填充所有点
std::vector<Point2f> CoveragePlannerBase::fillAllPoints(vector<Point2f> points, double interval){
    std::vector<Point2f> filled_points;
    for (size_t i = 0; i < points.size()-1; ++i){
        std::vector<Point2f> segmentPoints = generateLinePoints(points[i], points[i + 1], interval);
        filled_points.insert(filled_points.end(), segmentPoints.begin(), segmentPoints.end());
    }  
    if(points.size() > 2){  //填充终点到起点数据
        std::vector<Point2f> segmentPoints = generateLinePoints(points.back(), points.front(), 0.05);
        filled_points.insert(filled_points.end(), segmentPoints.begin(), segmentPoints.end());
        
    }
    return filled_points;  
}

// 坐标转换
cv::Point CoveragePlannerBase::Word2Opencv(geometry_msgs::Point point_2f){
    // cout << "x: " << point_2f.x << ", y: " << point_2f.y << endl;
    cv::Point point(
        std::round((point_2f.x - mapHandlerPtr_->getOrigin().x) / mapHandlerPtr_->getResolution()), 
        std::round((point_2f.y - mapHandlerPtr_->getOrigin().y) / mapHandlerPtr_->getResolution())
    );
    point.y = mapHandlerPtr_->getHeight() - 1 - point.y;
    return point;
}

geometry_msgs::Point CoveragePlannerBase::Opencv2World(cv::Point point){
    geometry_msgs::Point point_2f;
    point_2f.x = mapHandlerPtr_->getOrigin().x + (point.x + 0.5) * mapHandlerPtr_->getResolution();
    point_2f.y = mapHandlerPtr_->getOrigin().y + (mapHandlerPtr_->getHeight() - 1 - point.y + 0.5) * mapHandlerPtr_->getResolution();
    return point_2f;
}


};  //namespace